About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Adding Force Control Capabilities

Description: With some special settings, you can enable a Cartesian impedance control component of hrpsys controller called "Impedance Control" on HiroNxo robots, which utilizes the realtime nature of QNX.

Keywords: force sensor, impedance control, hrpsys controller

Tutorial Level: ADVANCED

Next Tutorial: Setup ROCON App Manager

Adding Impedance Control

Impedance control using hrpsys

Use the output from force/torque sensors attached at the end effectors to achieve arms' compliance.

Hiro robot reacting using impedance control feature of hrpsys. More about this video can be found here.

To realize the smooth movement, control loop is closed at the low-level controller on ControlBox/QNX. This requires sensors connected to ControlBox.

Requirement

• Force/torque sensor, and its driver software that is compatible with QNX 6.5.0.
  • So far, the large part of this tutorial page is written based on the achievement with the following sensors. No precompiled QNX drivers are provided for neither of them as of June 2016:

    • JR3 device (In Japan, this product is discontinued as of June 2016).

    • Dynpick WDF-6M200-3 (Discontinued)

  • F/T sensor's stable output. This is crucial because robot can easily go wild while impedance is on with unstable sensor output! This often requires:

    • Power supply to the sensor by external source (i.e. not by the ControlBox).

    • Keep the the sensor's cable away from other cables, esp. those that emit wireless signals.

  • (For Dynpick) By May 2017, QNX 6.5.0 Service Pack 1 is required to establish the connection between the sensor and the host.

• Appropriately change hrpsys controller on QNX.

  • QNX developer's license, if aforementioned device driver is not precompiled.

Detailed steps for doing this installation is not provided publicly to avoid undesired result and end users are NOT advised to try by themselves. Contacting professional service providers is recommended.

Usage: De/Activate impedance feature via Python client

Since Impedance Control feature is deactivated by default, you need to explicitly activate it per each sensor. You can do so via the same Python client that we always use.

$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py
: (initialize ipython terminal)
IN[1]: robot.startImpedance('rarm')
IN[2]: robot.startImpedance('larm')

Also you can deactivate it by:

IN[3]: robot.stopImpedance('rarm')
IN[4]: robot.stopImpedance('larm')

WARNING: Be sure to deactivate Impedance Control feature when the robot is not in use! Arms can move based on the sensors output even without any commands passed while Impedance is on.

Every day before you activate impedance, check the following minimum set of parameters are fine-tuned. See the following section for the parameters that need to be reconfigured.

Some utility functions are defined in hironx_client.py.

Impedance Controller Parameters

Minimum set of parameters

Arms movement depends on the sensor output, so all users must find the set of parameters that work the best for you. In particular you can start with the following:

• "mass/damper/spring" defines the stiffness.

• reference force to compensate the force in XYZ direction.

  • Passing this varies per hrpsys version, which is explained in the later section.

  • Setting this 'may' be automated (there's an open pull request).

Minimum example (DON'T just use this. You MUST find the values for your robot every day):

robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [-0.56, -0.23, 0.940])
robot.startImpedance('rarm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [1.543, 0.13, 2.809])

Full parameters

All parameters available are defined in hrpsys doc about ImpedanceControllerService. You can set most of those parameters via startImpedance method (api doc), and some can be set via another method, which will be explained later. In the following, for example, mass/damper/spring parameters are passed for the left arm.

IN[2]: robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0)

• Parameters for the impedance controller passed via Python I/F here are processed in ImpedanceController RT component. For example, reference force mentioned above is processed here (referring to hrpsys 315.1.7).

NOTE: Although all users can call the same method startImpedance, the arguments slightly vary depending on the version of hrpsys controller in use. API doc for each version:

• 315.1.x

• 315.2.x

• 315.3.x or higher.

hrpsys `315.1.x`

The method with all available arguments can look like:

• startImpedance_315_1(self,
                       arm,
                       M_p = 100.0,
                       D_p = 100.0,
                       K_p = 100.0,
                       M_r = 100.0,
                       D_r = 2000.0,
                       K_r = 2000.0,
                       ref_force = [0, 0, 0],
                       force_gain = [1, 1, 1],
                       ref_moment = [0, 0, 0],
                       moment_gain = [0, 0, 0],
                       sr_gain = 1.0,
                       avoid_gain = 0.0,
                       reference_gain = 0.0,
                       manipulability_limit = 0.1)

hrpsys `315.2.x` or higher

From 315.2.0 onward, following arguments are dropped and can be set by robot.seq_svc.setWrenches instead of startImpedance method. See an example at hrpsys sample code:

• ref_force[fx, fy, fz] (unit: N) and ref_moment[tx, ty, tz] (unit: Nm) can be set via self.seq_svc.setWrenches. For example:

  • self.seq_svc.setWrenches([fx_r, fy_r, fz_r, tx_r, ty_r, tz_r,
                              fx_l, fy_l, fz_l, tx_l, ty_l, tz_l,])

    NOTE: In the above example right arm comes first and then left in setWrenches arg. This order of sensors is set in the config so can vary per robot. Make sure you know the order (the one who installs this whole thing should tell you about it).

Migration from hrpsys `315.1.x`, and `315.2.x` or higher

For example, setting reference force must be done as follows:

• 315.1.x

  • robot.startImpedance('larm', ref_force = [-0.56, -0.23, 0.940])

• 315.2.x or higher

  • robot.startImpedance('larm')
    robot.seq_svc.setWrenches([0, 0, 0, 0, 0, 0,
                               -0.56, -0.23, 0.940, 0, 0, 0,])

Monitor the force sensor output values

Monitor the output via ROS on Ubuntu

You can monitor the F/T sensor's input values via ROS. Be sure you run RTM-ROS bridge (see a previous tutorial):

$ rostopic echo /lhsensor    % Left hand
$ rostopic echo /rhsensor    % Right hand

You can plot these same ROS topics on rqt_plot (e.g. type in "/lhsensor/wrench/force" in the topic name text field for the left arm. 2 rqt_plot plugins are opened in the following picture.):

See the impedance parameters currently set

You can check the current parameters via getImpedanceControllerParam method in Python. Do not forget ic_svc object.

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py --host nextage
:
In [3]: robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [-0.55, -0.24, 1.025])

In [5]: robot.ic_svc.getImpedanceControllerParam('larm')
Out[5]:
(False,
OpenHRP.ImpedanceControllerService.impedanceParam(name='', base_name='', target_name='', M_p=0.0, D_p=0.0, K_p=0.0, M_r=0.0, D_r=0.0, K_r=0.0, ref_force=[7.02753183846418e+199, 0.07999999984204262, 0.0], force_gain=[0.0, 0.0, 0.0], ref_moment=[1.0, 1.0, 1.0], moment_gain=[0.0, 0.0, 0.0], sr_gain=0.0, avoid_gain=0.0, reference_gain=0.0, manipulability_limit=0.0))

(Advanced) Monitor the output via the sensor driver directly

Only if you have login access to the ControlBox (QNX), the following may be an option for you in addition to what's available via ROS.

• For Dynpick, you can run the sensor driver's sample value checker:

  $ /opt/jsk/bin/sample_dynpick
  client: msg_no: _IO_MAX + 4
  client: msg_reply:
  7|
  client: msg_reply:
  7|
  client: msg_reply:
  7|
  client: msg_reply:
  7|
  client: msg_reply:   1.519  -0.026   1.677   1.093   2.146  -0.381
                      -0.597  -0.030   1.137   0.079   0.490  -0.513
  client: msg_reply:   1.509  -0.029   1.681   1.093   2.143  -0.371
                      -0.597  -0.027   1.137   0.081   0.492  -0.515
  client: msg_reply:   1.504  -0.026   1.677   1.092   2.151  -0.371
                      -0.600  -0.026   1.131   0.082   0.492  -0.519
  :

  # The order of the output:
  • For each line, force and rotation about XYZ axes.

  • Lines are aligned in the order that defined in the robot's 3D model (VRML format) as sensorID attribute.

Impedance on simulator

(TBD)

Trouble shooting

{l, r}hsensor not being published?

Possible causes:

• ROS master may not be running --> Check a previous tutorial to see how to run ROS master.

• F/T sensor driver may not be working as expected.
• F/T sensor driver may not be connected as expected.

Ask here with the output of the following:

$ ssh %YOUR_QNX_USER%@nextage
(Password: YOUR_QNX_USER's)

$ usb -vvv
$ ls -l /dev/ser*
$ sloginfo
$ ps -ef | grep serusb
$ cat /opt/jsk/bin/sample_dynpick
$ cat /opt/jsk/bin/run_dynpick.sh

Other than hrpsys

You can also realize force/torque control on Ubuntu machine side if your application doesn't require cut-throat real-time interacton. Some HiroNxo users have successfully impelemented Dynpick force sensor (Wacoh Tech).

Weight offset

The are multiple ways to to cancel the weight offset of the robot's links from force/torque sensor's value.

Using hrpsys

If weight of eef is known

You can use "Remove Force Sensor Link Offset" RTC (called AbsoluteForceSensor for hrpsys 315.1.x), along with "Impedance Control" RTC.

• [Advantage] This method is not restricted by the orientation of the eef.
• You need to pass parameters such as the weight of eef, center of mass, moment of inertia.

1. Check if Impedance Controller (IC) RTC is enabled on your robot. Run Hiro / NXO python script (as you always do), then find portions similar to the following:

• [rtm.py] Connect sh.rhsensorOut - ic.ref_rhsensorIn
  [rtm.py] Connect sh.lhsensorOut - ic.ref_lhsensorIn
  [rtm.py] Connect HiroNX(RobotHardware)0.q - rmfo.qCurrent
  [rtm.py] Connect rmfo.off_rhsensor - ic.rhsensor
  [rtm.py] Connect rmfo.off_lhsensor - ic.lhsensor
  [rtm.py] Connect HiroNX(RobotHardware)0.q - ic.qCurrent
  [rtm.py] Connect sh.basePosOut - ic.basePosIn

  RobotHardware.{r, l}hsensor should exist. If not, you should look at the return values of rtls command, then in the output find IC or something similar:

  Ubuntu$ rosrun rtshell rtls %HOST_ROBOT%:15005/

  Or in newer version of software,

  Ubuntu$ rtls %HOST_ROBOT%:15005/

2. If IC RTC is connected to RMFO RTC (as the example above does), you can

• //  Obtaining a structure that stores offset parameters.
  In [13]: a = robot.rmfo_svc.getForceMomentOffsetParam('lhsensor')[1]
  
  // Setting mass to a member variable of the obtained structure.
  In [14]: a.link_offset_mass = 0.5
  
  // Set the structure back.
  In [15]: a = robot.rmfo_svc.setForceMomentOffsetParam('lhsensor', a)
  
  // If you're curious, this is how the offset parameter structure looks.
  In [16]: a
  Out[16]: OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0.0, 0.0, 0.0], moment_offset=[0.0, 0.0, 0.0], link_offset_centroid=[0.0, 0.0, 0.0], link_offset_mass=0.0)

  With the example above, only weight is given for the brevity. It should work but more parameters as noted above are necessary for the better result.

If weight of eef NOT known

With the steps below you can record an offset at an arbitrary pose and then subtract it out.

TBD

Using a ROS package

Use force_torque_tools

force_torque_tools offers a capability to conduct calibration for your force/torque sensor, and provides a detailed tutorial. Following is the tailored version of instruction for HiroNXO, based on the same tutorial.

• *NOTE-1**: Be sure to consult the original tutorial whenever you have questions; the following is only a supplement to it.

• *NOTE-2**: As of today (Aug 2015) this is NOT tested with real hardware. Use at your own risk and feedback is welcomed (modifying this wiki is appreciated if you find correct descriptions. Otherwise open a issue ticket at ROS software repository of NEXTAGE OPEN.

1. Copy a config from force_torque_sensor_calib package. Here we use nextage_ros_bridge pkg to store the config.

   $ roscp force_torque_sensor_calib example_ft_calib_params.yaml `rospack find nextage_ros_bridge`/conf
   $ roscd nextage_ros_bridge/conf && mv example_ft_calib_params.yaml nxo_ft_calib_params.yaml

2. Edit the config file.

   - loop_rate: 650.0
   + loop_rate: 1000

   At the time of writing, the authoer is thinking of dynpick_driver as a f/t sensor, which [uses 1,000 hz](https://github.com/tork-a/dynpick_driver/blob/b47b634c4a2b78f69f5d9699bb5c4ce98d44682f/launch/run.launch#L3). Choose appropriate value for your sensor.

    moveit_group_name: 'left_arm'
   + moveit_group_name: 'right_arm'

   Depending on which arm-tip of NEXTAGE OPEN you attach the f/t sensor, specify the group of MoveIt!.

   - random_poses: false
   + random_poses: true
   
   # number of random poses
   - number_random_poses: 0
   + number_random_poses: 30

   As the tutorial recommends, better to use random poses unless you have specific reasons to not to.
3. Copy and modify the launch file.

   $ roscp force_torque_sensor_calib example_ft_calib.launch `rospack find nextage_ros_bridge`/launch
   $ roscd nextage_ros_bridge/launch && mv example_ft_calib.launch nxo_ft_calib.launch

   Edit:

   -    <rosparam command="load" file="$(find force_torque_sensor_calib)/config/example_ft_calib_params.yaml"/>
   +    <rosparam command="load" file="$(find nextage_ros_bridge)/conf/nxo_ft_calib_params.yaml"/>

4. Run calibration.

   roslaunch nextage_ros_bridge nxo_ft_calib.launch

Once again, always go see the original tutorial for the detail.